-
Notifications
You must be signed in to change notification settings - Fork 16.8k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
AP_InertialSensor: Rework rotation handling #5216
Conversation
e576e15
to
ed36212
Compare
hi Julien, this is a nice re-work, but it will need a lot of testing on different boards. I did a spot-check on a Pixhawk1 and found the 2nd accel ends up with the wrong rotation. The PR changes the LSM9DS0 driver gyro orientation but not the accel orientation. I think others are wrong too. |
I just checked master and it looks like we have an existing error in the l3gd20 gyro orientation from my sensor re-work. I'll work on fixing that now. |
i've fixed the l3gd20 rotation, and removed the raspilot hack in the driver. It turns out we need a different rotation for the gyro than for the accel. I had assumed they were the same in my recent changes. |
@tridge I don't think that it is possible that we need a different rotation for the accels and the gyros. |
ed36212
to
270f37d
Compare
fixed the rotation for accel in L3G and LSM drivers. |
3560066
to
95e247f
Compare
Rebased. Tested OK on pixhawk. |
I've tested pixhawk2, pixracer, pixfalcon, phmini raspilot. There were a number of fixes needed: |
95e247f
to
57b0b96
Compare
Sorry for wasting your time. They are leftovers and a mistake for raspilot. My assumptions regarding rotation changes in the PR message are right. |
57b0b96
to
3ba7be5
Compare
@tridge I added your corrections to my commit for clarity purpose. If you prefer, I could just push them on my pr branch with the previous version. |
I don't mind either way, just need to do more testing :-) |
@lucasdemarchi You said you could test that PR with some boards. I am waiting some more tests before rebasing. Would you have time for that? |
@jberaud Is this something you'd still like to work on? It needs a big rebase and I think that if you aren't able to rework it there is no point in keeping it open. |
I can rebase again, but since there are some boards that I don't have, I won't be anymore able to test this PRs on them. So there are 3 solutions : |
I think this is a very worthwhile PR and from comments above I think @tridge does too. I think we can get people to test the boards - coming up with a test procedure for people to follow might help with that. |
Hi, sorry for the long delay. We do need to test it, but I think getting it mathematically correct would allow us to be more confident on applying it even if we can't test it everywhere.
This actually means ROTATION_ROLL_180_YAW_270 since it's and intrinsic xyz rotation. You can also check on Vector3::rotate() that what I just said matches the code. Manually checking some of them: PXF: roll180, yaw270, roll180, yaw270 = yaw180 -> correct on patch The checks above were made manually. I think I had at some point a script using sympy to work out the rotations. If I don't find it I'll derive the rotations again so we can be more confident they are correct. |
@lucasdemarchi Thanks very much for your inputs. Since I have tested that on Bebop, there must be something in my params that I was assuming wrongly. I'll check that, fix the patch, and rebase. Thanks again. |
@lucasdemarchi I have checked the code in vector3 and here is what I see Could you explain why it should be ROTATION_ROLL_180_YAW_270 ? |
Oh... I always forget the convention we are using... It's zyx, not xyz, i.e. yaw, pitch and roll in order. We need to add a comment on rotations.{h,CPP}. Now to check the others places |
@jberaud I rechecked all the rotations you changed with the following test program: https://gist.github.com/d049489154f4f90141644b3c90403009 All of them are correct. Could you rebase this? |
2674f8c
to
382e6f3
Compare
@lucasdemarchi Done. |
i'm quite nervous about this patch - it is very hard to validate it fully. We thought it was right before and it turned out to be wrong. |
@jberaud @lucasdemarchi Can we come up with some steps to test this PR? I can bother people to test boards, but without a testing procedure, it is hard. |
@OXINARF Step tests should consist in determining if the orientation of the Accel and Gyro are correct. Then to check the gyro orientation, graph gyro (ggyro in mavproxy with mavinit.scr script for primary gyro or ggyro2 for secondary gyro). Then take the board in your hand facing its right side (Y axis). Turn it clockwise and the Y axis should go negative. Then take the board in your hand facing its bottom (Z axis). Turn it clockwise and the Z axis should go negative. Then to check the accel orientation, graph accel (gaccel for primary and gaccel2 for secondary, etc...). Then turn it and then, put it on its right side (Y axis facing the table). Y should be negative. Then turn it, put it nose down (X axis facing the table). X should become negative. Note: When I say "Right" side, I am in the regular drone coordinate system, meaning drone facing straight ahead, not pointing towards you. |
@tridge I understand your concerns. I have seen master being broken numerous times on Bebop but I guess it is because it is quite an uncommon board for ardupilot. I hope we can test this PR correctly enough to merge it, because in the end, it is all about testing right. |
Right now, MPU based imus perform the rotations in the driver in an implicit way when converting sensor bus data to actual x, y, z data. MPU6000 and MPU9250 : x = y, y = x, z = -z meaning ROTATION_ROLL_180_YAW_90 LSM9DS0 and L3G4200 : x = x, y = -y, z = -z meaning ROTATION ROLL_180 This commit removes these implict rotations from the drivers and modify the per-board orientations so the behaviour doesn't change, by applying a combination of ROLL_180_YAW_90 and then the former board rotation for MPU9250 and MPU6000 and applying a combination of ROLL_180 and the former board rotation for LSM9DS0 and L3G4200. RASPILOT had a compilation flag inside the LSM9DS0 driver to add a YAW_90 rotation. When added to the ROLL_180 applied when converting data, it gives the same rotation as the computed rotation for the mpu6000, i.e ROLL_180_YAW_90 Checks: Leftovers and mistakes fixed by tridge included Tested by lucasdemarchi with https://gist.github.com/d049489154f4f90141644b3c90403009
Some rotations were applied on the raw data from the sensors in an implicit way. This PR solves this as described after. It makes the integration on a new board a bit more straightforward and the drivers get a bit easier to understand.
Tested on
Bebop
Disco
Pixhawk 1
pixhawk2, pixracer, pixfalcon, phmini raspilot (by Tridge)
so it needs to be carefully reviewed and tested on other boards.
Right now, MPU based imus perform the rotations in the driver in an
implicit way when converting sensor bus data to actual x, y, z data.
MPU6000 and MPU9250 :
x = y, y = x, z = -z
meaning ROTATION_ROLL_180_YAW_90
LSM9DS0 and L3G4200 :
x = x, y = -y, z = -z
meaning ROTATION ROLL_180
This commit removes these implict rotations from the drivers and modify the
per-board orientations so the behaviour doesn't change, by applying a
combination of ROLL_180_YAW_90 and then the former board rotation for MPU9250
and MPU6000 and applying a combination of ROLL_180 and the former board rotation
for LSM9DS0 and L3G4200.
RASPILOT had a compilation flag inside the LSM9DS0
driver to add a YAW_90 rotation. When added to the ROLL_180 applied when
converting data, it gives the same rotation as the computed rotation for the
mpu6000, i.e ROLL_180_YAW_90
The rotation combinations I have calculated for reference:
Applying ROTATION_ROLL_180_YAW_90:
then ROTATION_PITCH_180 => ROTATION_YAW_90
then ROTATION_ROLL_180_YAW_90 => ROTATION_NONE
then ROTATION_ROLL_180_YAW_270 => ROTATION_YAW_180
then ROTATION_YAW_270 => ROTATION_ROLL_180
then ROTATION_YAW_90 => ROTATION_PITCH_180
then ROTATION_ROLL_180 => ROTATION_YAW_270
then ROTATION_PITCH_180_YAW_90 => ROTATION_YAW_180